#include "stepmotor.h"

/*电机相关定义*/
#define MOTOR_DIR       (0)         // 电机正方向, 可选 0 正转 1 反转
#define MOTOR_VEL       (5000)      // 1 2 5 6 号步进电机运动速度
#define MOTOR_VEL_3_4   (800)       // 3 4 号步进电机运动速度

MotorPose_t MotorPose = {0};

/**
* @brief    等待电机回复
* @param    ms：超时时间ms, 0表示一直等待
* @return : 1-超时未收到回复, 0-表示收到回复
*/
u8 waitSMResponse(u16 ms)
{
    u16 i = 0;
    while(can.rxFrameFlag == false)
    {
        if(ms != 0 && i >= ms){
            return 1; 
        }
        delay_ms(1);
        i++;  
    }
    can.rxFrameFlag = false;
    return 0;
}

/// @brief 更新电机位置
/// @param  send: 是否上报给上位机 1上报 0不上报
void motorUpdatePose(u8 send)
{
    u8 res;
    
    #define GP(id) \
    Emm_V5_Read_Sys_Params(id, S_CPOS); \
    res = waitSMResponse(50); \
    if(res == 0 && can.CAN_RxMsg.Data[0] == 0x36 && can.CAN_RxMsg.DLC == 7) { \
        MotorPose.M##id[0] = (u8)id; \
        MotorPose.M##id[1] = can.CAN_RxMsg.Data[1]; \
        MotorPose.M##id[2] = can.CAN_RxMsg.Data[2]; \
        MotorPose.M##id[3] = can.CAN_RxMsg.Data[3]; \
        MotorPose.M##id[4] = can.CAN_RxMsg.Data[4]; \
        MotorPose.M##id[5] = can.CAN_RxMsg.Data[5]; \
        if(send == 1){\
            sendPacket(0, CMD_MOTOR_GETPOS, &MotorPose.M##id[0], 6);\
        }\
    }

    GP(1)
    GP(2)
    GP(3)
    GP(4)
    GP(5)
    GP(6)
    
    /*更新舵机角度*/
//    u8 sa[2] = {7, MotorPose.S1};
//    sendPacket(0, CMD_MOTOR_GETPOS, &sa[0], 2);
//    sa[0] = 8;
//    sa[1] = MotorPose.S2;
//    sendPacket(0, CMD_MOTOR_GETPOS, &sa[0], 2);
    
    #undef GP
}

/// @brief 用于控制电机位置
/// @param data 
/// data[0] : 电机编号 [1,6]
/// data[1]--data[4] : 电机角度 (大端)
/// @param len 
/// @return 
int motorPoseCmd(char *data, int len)
{
    if(data[0] < 1 || data[0] > 6 || len != 5)return -1;
    
    uint8_t motorid = data [0];
    
    uint8_t res;
    
    /*解析角度*/
    uint32_t pose = 0;
    for(uint8_t i = 1; i<5; i++)
    {
        pose |= (uint32_t)data[i] << ((4-i)*8);
    }

    if(data[0] == 3 || data[0] == 4)
    {
        /*3号和4号电机要同步转动*/
        delay_ms(50);
        Emm_V5_Pos_Control(3, MOTOR_DIR, MOTOR_VEL_3_4, 0, pose, true, true);
        res = waitSMResponse(50); if(res == 1) return -1;
        delay_ms(50);
        Emm_V5_Pos_Control(4, MOTOR_DIR, MOTOR_VEL_3_4, 0, pose, true, true);
        res = waitSMResponse(50); if(res == 1) return -1;
        delay_ms(50);
        Emm_V5_Synchronous_motion(0);
        res = waitSMResponse(50); if(res == 1) return -1;
        delay_ms(50);

    }else
    {
        /*控制其他电机*/
        Emm_V5_Pos_Control(motorid, MOTOR_DIR, MOTOR_VEL, 0, pose, true, false);
        res = waitSMResponse(50); if(res == 1) return -1;
        
    }
    
    return 0;
    
}

/// @brief 用于控制舵机
/// @param data data[0]表示舵机编号:7/8, data[1]表示舵机角度
/// @param len 固定为2
/// @return 
int motorServo(char *data, int len)
{   
    if(len != 2)return -1;
    if(data[1]>180)data[1] = 180;
    switch(data[0]){
        case 7: Servo1_SetAngle((float)data[1]); break; // 360 舵机
        case 8: Servo2_SetAngle((float)data[1]); break;
        default: return -1;
    }
    return 0;
}




